#include "PID.h"
#include "timer.h"


//低通参数
// low pass filter:           0.079577472903393
// f_cut = 1/(2*PI*cutoff_freq)
// f_cut = 2 Hz -> _filter = 79.5774e-3
// f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter =  7.9577e-3
// f_cut = 25 Hz -> _filter =  6.3662e-3
// f_cut = 30 Hz -> _filter =  5.3052e-3
volatile float lowpass_filter = 7.9577e-3;


/*限幅函数*/
float Limits_data(float LimitsData,float LimitsUp,float LimitsDown)
{
	if(LimitsUp==0 && LimitsDown==0){
		return LimitsData;
	}
	if(LimitsData > LimitsUp)
	{
		LimitsData = LimitsUp;
	}
	if(LimitsData < LimitsDown)
	{
		LimitsData = LimitsDown;
	}
	return LimitsData;
}

/**
 * @brief PID运算器
 * @param erro:误差
 * @param last_time：上一次计算的时间戳
 * @param pid：pid运算结构体、包含了PID参数
 * @param pidout：存放pid运算结果
 */
void pid_calculate(PID_Erro *erro,unsigned int *last_time,PID *pid,PIDOut *pidout)
{
	float lastErro = erro->Last_Erro;
	float last_Delta_Erro = erro->Last_Erro_Delta;
	float cur_erro = erro->Erro;
	
	float last_Time = *last_time;
	
	unsigned int curTime = get_micros(); //获取本次内环控制的时间戳，计算微分时间
	float dt = (last_Time > 0) ? ((curTime - last_Time) / 1000000.0f) : 1;
	
	//计算误差微分
	float delta_erro = (cur_erro-lastErro)/dt;
	//误差微分20hz低通
	delta_erro = last_Delta_Erro + (dt / (lowpass_filter + dt)) * (delta_erro - last_Delta_Erro);
	//更新数据
	erro->Last_Erro_Delta = delta_erro;
	erro->Last_Erro = cur_erro;
	*last_time = curTime;
	//计算PID 输出
	pidout->pOut = pid->Kp * cur_erro;
	pidout->dOut = pid->Kd * delta_erro;
	pidout->iOut += pid->Ki * cur_erro;
	
	//积分微分限制
	pidout->iOut=Limits_data(pidout->iOut,pidout->iOut_limit_max,pidout->iOut_limit_min);
	pidout->dOut=Limits_data(pidout->dOut,pidout->dOut_limit_max,pidout->dOut_limit_min);
	
	pidout->value = pidout->pOut + pidout->dOut + pidout->iOut + pidout->feedforwardOut;
	//pid输出限制
	pidout->value=Limits_data(pidout->value,pidout->limit_max,pidout->limit_min);
	
}
